﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using i4pro_gruppe_d.Hardware;
using Antlr.Runtime;
using Antlr.Runtime.Tree;
using i4pro_gruppe_d.Types;

namespace i4pro_gruppe_d.ProcessManagement
{
    public class ProcessManagement
	{
		#region Members
		private Conveyer myConveyer;
    	private Gripper myGripper;
    	private LightSensor myLightSensor;
    	private Robotarm myRobotarm;
    	private Scale myScale;
    	private Utility myUtility;
        private Conveyer conveyer = new Conveyer(false);
        private Gripper gripper = new Gripper(false);
        private LightSensor lightSensor = new LightSensor(false);
        private Robotarm robotarm = new Robotarm(false);
        private Scale scale = new Scale(false);
        private Utility utility = new Utility(false);
        private const string CRLF = "\r\n";
        private const string defaultVector = "defVector";
        private int pointCount = 1;
        private Brick currentBrick = new Brick();
		#endregion

		#region Contruction
		public ProcessManagement()
		{
			myConveyer = new Conveyer(false);
			myGripper = new Gripper(false);
			myLightSensor = new LightSensor(false);
			myRobotarm = new Robotarm(false);
			myScale = new Scale(false);
			myUtility = new Utility(false);
		}
		#endregion

		#region Commands
		public void TakeCommand(string cmd)
		{
			switch (cmd)
			{
				case "start":
					Start();
					break;
			}
		}

        private void sleep()
        {
            System.Threading.Thread.Sleep(500);
        }

		private void Start()
		{
			// Init
			myUtility.InitRobot();
			myUtility.IsRobotOnline();

			// Flyt klodsen til position
			myConveyer.Start();
			myLightSensor.ActivateLightsensor();
			myLightSensor.DeactivateLightsensor();
			myConveyer.Stop();
			
			// Grib om klodsen
			myRobotarm.MoveTo("Simulator 1 - Flyt til klods", 1);
			myGripper.Open();
			myRobotarm.MoveTo("Simulator 1 - Ned over klods", 1);
			myGripper.Close();

			// Mål klodsen
			myGripper.MeasureWidth();
			myGripper.Open();
			myRobotarm.MoveTo("Simulator 1 - Over klods på anden dimension", 1);
			myGripper.Close();
			myGripper.MeasureWidth();
			myRobotarm.MoveTo("Simulator 1 - Flyt til vægt", 1);
			myGripper.Open();
			// myScale.VEJ!!
			myRobotarm.MoveTo("Simulator 1 - Over klods på tredje dimension", 1);
			myGripper.Close();
			myGripper.MeasureWidth();

			// Færdiggør
			myRobotarm.MoveTo("Simulator 1 - Flyt til sorteret destination", 1);
			Console.WriteLine("SUCCES SUCCES SUCCES");
			Console.WriteLine("SUCCES SUCCES SUCCES");
			Console.WriteLine("SUCCES SUCCES SUCCES");
			Console.WriteLine("SUCCES SUCCES SUCCES");
		}
		#endregion        


        public void DFSRecursiveRunThoughOfAST(CommonTree tree)
        {
            if (tree.Children != null)
            {
                foreach (var child in tree.Children)
                {
                    var childTree = child as CommonTree;

                    if (childTree.Text != CRLF)
                    {
                        DFSRecursiveRunThoughOfAST(childTree);

                        switch (childTree.Text)
                        {
                            case "startTB":
                            {
                                conveyer.Start();
                                break;
                            }
                            case "stopTB":
                                {
                                    conveyer.Stop();
                                    break;
                                }
                            case "home":
                                {
                                    utility.HomeRobot();
                                    break;
                                }
                            case "init":
                                {
                                    utility.InitRobot();
                                    utility.SetControl('&', true);

                                    utility.DefineVector('A', defaultVector, 100);

                                    Vector aboveConveyerVector = new Vector
                                                                     {
                                                                         x = 237810,
                                                                         y = 79960,
                                                                         z = 172350,
                                                                         pitch = -90000,
                                                                         roll = 20000,
                                                                         type = -32766,
                                                                         pointNumber = pointCount++
                                                                     };
                                    utility.AddCoordinatePoint(defaultVector, aboveConveyerVector);

                                    Vector onConveyerVector = new Vector
                                                                  {
                                                                      x = 251290,
                                                                      y = 80110,
                                                                      z = 107160,
                                                                      pitch = -90000,
                                                                      roll = 20000,
                                                                      type = -32766,
                                                                      pointNumber = pointCount++
                                                                  };
                                    utility.AddCoordinatePoint(defaultVector, onConveyerVector);

                                    robotarm.WatchMotion(true, false);

                                    break;
                                }
                            case "mtWeight":
                                {
                                    //robotarm.MoveTo(defaultVector, x);
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }
                            case "mtConveyer":
                                {
                                    robotarm.MoveTo(defaultVector, 1);
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }
                            case "mtSortbox":
                                {
                                    //robotarm.MoveTo(defaultVector, x);
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    //add brick to database
                                    currentBrick = new Brick();
                                    break;
                                }
                            case "grapBrick":
                                {
                                    if (currentBrick.Time == new DateTime())
                                        currentBrick.Time = DateTime.Now;

                                    gripper.Open();
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    //move down relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    gripper.Close();
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    //move up relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    break;
                                }
                            case "releaseBrick":
                                {
                                    //move down relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    gripper.Open();
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    //move up relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    gripper.Close();
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }

                                    break;
                                }
                            case "measureWidth":
                                {
                                    currentBrick.Width = gripper.MeasureWidth();
                                    break;
                                }
                            case "measureHeight":
                                {
                                    currentBrick.Height = gripper.MeasureWidth();
                                    break;
                                }
                            case "measureLength":
                                {
                                    currentBrick.Length = gripper.MeasureWidth();
                                    break;
                                }
                            case "weighBrick":
                                {
                                    int tempWeight = 0;
                                    scale.MeasureWeight(ref tempWeight);
                                    currentBrick.Weight = tempWeight;
                                    break;
                                }
                            case "rollGrapperLeft":
                                {
                                    //roll relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }

                            case "rollGrapperRight":
                                {
                                    //roll relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }
                            case "pitchGrapperUp":
                                {
                                    //pitch relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }
                            case "pitchGrapperDown":
                                {
                                    //pitch relative
                                    sleep();
                                    while (!robotarm.CheckMotionStatus())
                                    {
                                    }
                                    break;
                                }
                            case "hvis":
                                {
                                    if (tree.Children != null)
                                    {
                                        var hvisChildTree = tree.Children[0] as CommonTree;

                                        if (hvisChildTree.Text != CRLF)
                                        {
                                            switch (hvisChildTree.Text)
                                            {
                                                case "==":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);

                                                        if (a == b)
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);
                                                            
                                                        }
                                                        break;
                                                    }
                                                case "!=":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);
                                                        
                                                        if (a != b)
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);

                                                        }
                                                        break;
                                                    }
                                                case ">=":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);

                                                        if (a >= b )
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);

                                                        }
                                                        break;
                                                    }
                                                case "<=":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);

                                                        if (a <= b)
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);

                                                        }
                                                        break;
                                                    }
                                                case "<":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);

                                                        if (a < b)
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);

                                                        }
                                                        break;
                                                    }
                                                case ">":
                                                    {
                                                        int a, b;
                                                        int.TryParse(hvisChildTree.Children[0].ToString(), out a);
                                                        int.TryParse(hvisChildTree.Children[1].ToString(), out b);
                                                        

                                                        if (a > b)
                                                        {
                                                            var hvisSubtree = tree.Children[1] as CommonTree;
                                                            DFSRecursiveRunThoughOfAST(hvisSubtree);

                                                        }
                                                        break;
                                                    }
                                            }
                                        }
                                    }
                                    break;
                                }
                        }

                    }
                    }
            }
        }
	}
}
